#include "../include/Deplacement.h"

Deplacement::Deplacement(Entite *e, Position p, Carte* c)
{
	carte = c;
	entite = e;
	dest = p;

    // SIMPLE WAY WORKING
	//checkpoints.push_back( dest);

    // PATHFINDING
    PathFinding pathF(c);
    checkpoints = pathF.trouverChemin( *e, p);
    // first checkpoint is the actual position of the entity
    checkpoints.erase( checkpoints.begin());

	fini = false;


	// test
	Bloc obstacle;

	#if VERBOSE
    if( obstacle.getTaille().x != 0 && obstacle.getTaille().y != 0 )
        std::cout<<"[DEPLACEMENT] Batiment sur le chemin "<<std::endl;
	std::cout<<"[DEPLACEMENT] Calcul de Deplacement Fini"<<std::endl;
    std::cout<<"[DEPLACEMENT] Path contains "<<checkpoints.size()<<" steps"<<std::endl;
	#endif
}

void Deplacement::executer()
{

    bool checkpointAtteint = false;

	float distanceVoulue =  (VITESSE_COEFF/100.0) * ((float)entite->getVitesse()*1.0);
	float eMilieuX = entite->getPosition()->x +  ( entite->getTaille().x /2) ;
	float eMilieuY = entite->getPosition()->y + (entite->getTaille().y /2);
	float tmp = ( eMilieuX - checkpoints[0].x )*( eMilieuX - checkpoints[0].x ); 
	tmp+= ( eMilieuY - checkpoints[0].y )*( eMilieuY - checkpoints[0].y ); 
	float distanceCheckPoint = sqrt( tmp);
	if( distanceCheckPoint <= distanceVoulue )
	{
		// on arrive au checkpoint
		/*Position p = checkpoints[0];
		p.x = p.x + ( entite->getTaille().x /2) ;
		p.y = p.y + ( entite->getTaille().y /2) ;
		entite->setPosition(p);	*/
		
        if( checkpoints.size() == 0 )
			fini = true;
        else
        {		
            checkpointAtteint = true;
      		checkpoints.erase( checkpoints.begin());

            	#if VERBOSE
	            std::cout<<"[DEPLACEMENT] Checkpoint atteint, plus que "<<checkpoints.size()<<" étapes"<<std::endl;
	            #endif
        }
		
	}
	else
    {
		// on peut encore avancer
		
		float angle = acos( ( checkpoints[0].x - eMilieuX )/ distanceCheckPoint);
			if( checkpoints[0].y < eMilieuY )
				angle = -angle;
		float tmp2 = eMilieuX -  checkpoints[0].x / distanceCheckPoint;
		Position nouvelle;
		nouvelle.x = entite->getPosition()->x+(distanceVoulue*cos(angle));
		nouvelle.y = entite->getPosition()->y+distanceVoulue*sin(angle);
		entite->setPosition(nouvelle);
    }
    
}

bool Deplacement::estFini()
{
	//return Carte::estProche( *entite, dest);
	return fini;
}

